/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-05-13     Administrator       the first version
 */
#include "encoder.h"

#define ENCODER_DEVICE_COUNT 4

/*编码器的结构体*/
encoder_state_t A_encoder, B_encoder, C_encoder, D_encoder;
static rt_device_t pulse_encoder_dev[ENCODER_DEVICE_COUNT] = {RT_NULL}; /* 编码器设备句柄数组 */

extern int32_t get_encoder_overflow_count(int index);


/*将编码器状态结构体初始化为默认值，四个编码器结构体*/
static void set_encoder_struct_to_default(encoder_state_t *encoder)
{
    encoder->Encoder = 0;
    encoder->total_count = 0;
    encoder->last_count = ENCODER_RELOAD_VALUE/2;   //mx里的还没有改
    encoder->overflow_count = 0;
    encoder->direction = 0;
    encoder->speed = 0.0f;
}

/* 编码器初始化 */
int Encoder_init(void)
{
    rt_err_t ret = RT_EOK;
    const char* encoder_names[ENCODER_DEVICE_COUNT] = { //设备名称，使用const可以避免被意外修改
        "pulse1",
        "pulse2",
        "pulse3",
        "pulse4"
    };
    rt_uint32_t i;

    // 查找设备、打开设备、使能编码器
    for (i = 0; i < ENCODER_DEVICE_COUNT; i++)
    {
        pulse_encoder_dev[i] = rt_device_find(encoder_names[i]);
        if (pulse_encoder_dev[i] == RT_NULL)
        {
            rt_kprintf("Error: Can't find %s device!\n", encoder_names[i]);
            return -RT_ERROR;
        }

        ret = rt_device_open(pulse_encoder_dev[i], RT_DEVICE_OFLAG_RDONLY);
        if (ret != RT_EOK)
        {
            rt_kprintf("Error: Open %s failed!\n", encoder_names[i]);
            return -RT_ERROR;
        }

        ret = rt_device_control(pulse_encoder_dev[i], PULSE_ENCODER_CMD_ENABLE, RT_NULL);
        if (ret != RT_EOK)
        {
            rt_kprintf("Error: Enable %s failed!\n", encoder_names[i]);
            return -RT_ERROR;
        }
    }

    rt_kprintf("encoder enable succeed\r\n");

//    rt_int32_t count;
    //     读取并打印编码器计数值
//            for (int index = 0; index <4; index ++)
//            {
//                rt_thread_mdelay(500);
                /* 读取脉冲编码器计数值 */
//            }
    /*设置编码器的初始值*/
//    __HAL_TIM_SET_COUNTER(&htim1, ENCODER_RELOAD_VALUE / 2);//30000，左前
//    __HAL_TIM_SET_COUNTER(&htim2, ENCODER_RELOAD_VALUE / 2);//左后
//    __HAL_TIM_SET_COUNTER(&htim3, ENCODER_RELOAD_VALUE / 2);//右前
//    __HAL_TIM_SET_COUNTER(&htim4, ENCODER_RELOAD_VALUE / 2);//右后

//    while(1)
//    {
//        rt_device_read(pulse_encoder_dev[0], 0, &count, 1);
//        /* 清空脉冲编码器计数值 */
//        rt_device_control(pulse_encoder_dev[0], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);
//        rt_kprintf("get count %d\n",count);
//        rt_kprintf("TIM1 count: %d\n", __HAL_TIM_GET_COUNTER(&htim1));
//    }

    /*设置完可以读取一次，看看是不是有真的变成30000了*/
//    rt_kprintf("TIM1 count: %d\n", __HAL_TIM_GET_COUNTER(&htim1));
//    rt_kprintf("TIM2 count: %d\n", __HAL_TIM_GET_COUNTER(&htim2));
//    rt_kprintf("TIM3 count: %d\n", __HAL_TIM_GET_COUNTER(&htim3));
//    rt_kprintf("TIM4 count: %d\n", __HAL_TIM_GET_COUNTER(&htim4));


//    /*初始化编码器结构体*/
//    set_encoder_struct_to_default(&A_encoder); //左前
//    set_encoder_struct_to_default(&B_encoder); //左后
//    set_encoder_struct_to_default(&C_encoder); //右后
//    set_encoder_struct_to_default(&D_encoder); //右前

    /* 正常关闭设备 */
//    exit_with_cleanup:
//    for (i = 0; i < 4; i++)
//    {
//        if (pulse_encoder_dev[i] != RT_NULL)
//        {
//            rt_device_close(pulse_encoder_dev[i]);
//        }
//    }
    return RT_EOK;
}

/* 读取编码器数据 */
void ReadEncoder(void)
{
//    while(1)
//    {
        rt_device_read(pulse_encoder_dev[0], 0, &A_encoder.Encoder, 1);//左前，向前走读出来是负值
        rt_device_control(pulse_encoder_dev[0], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);

        rt_device_read(pulse_encoder_dev[1], 0, &B_encoder.Encoder, 1);//左后，向前走读出来是负值
        rt_device_control(pulse_encoder_dev[1], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);

        rt_device_read(pulse_encoder_dev[3], 0, &C_encoder.Encoder, 1);//右后，正值
        rt_device_control(pulse_encoder_dev[3], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);

        rt_device_read(pulse_encoder_dev[2], 0, &D_encoder.Encoder, 1);//右前，正值
        rt_device_control(pulse_encoder_dev[2], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);

        // 单位是圈/秒
        //轮子转一圈的脉冲：(四倍频*编码器单圈脉冲*电机减速比)
        //轮子转了多少圈：(编码器差值/(四倍频*编码器单圈脉冲*电机减速比))
        //(编码器差值/(四倍频*编码器单圈脉冲*电机减速比))*测速频率（100Hz），即10ms
        //这个速度放大了1000倍
//        A_encoder.speed = 1000 * A_encoder.Encoder / (EncoderMultiples * Hall_13 * HALL_30F) * 100;
//        B_encoder.speed = 1000 * B_encoder.Encoder / (EncoderMultiples * Hall_13 * HALL_30F) * 100;
//        C_encoder.speed = 1000 * C_encoder.Encoder / (EncoderMultiples * Hall_13 * HALL_30F) * 100;
//        D_encoder.speed = 1000 * D_encoder.Encoder / (EncoderMultiples * Hall_13 * HALL_30F) * 100;

//        rt_kprintf("[EncoderTimer] Tick: %u | Speed A: %d B: %d C: %d D: %d\n",
//                   rt_tick_get(),
//                   (rt_int32_t)A_encoder.speed,
//                   (rt_int32_t)B_encoder.speed,
//                   (rt_int32_t)C_encoder.speed,
//                   (rt_int32_t)D_encoder.speed);

//        rt_kprintf("[EncoderTimer] Tick: %u | Speed A: %d B: %d C: %d D: %d\n",
//                   rt_tick_get(),
//                   (rt_int32_t)A_encoder.Encoder,
//                   (rt_int32_t)B_encoder.Encoder,
//                   (rt_int32_t)C_encoder.Encoder,
//                   (rt_int32_t)D_encoder.Encoder);

}
//    rt_int32_t counts[4] = {0};
//    for (index = 0; index <= 10000; index++)
//    {
//        rt_thread_mdelay(500);
//
//        /* 遍历所有编码器 */
//        for (i = 0; i < 4; i++)
//        {
//            /* 3. 读取计数值 */
//            rt_device_read(pulse_encoder_dev[i], 0, &counts[i], 1);
//
//            /* 4. 清空计数器 */
//            rt_device_control(pulse_encoder_dev[i],
//                             PULSE_ENCODER_CMD_CLEAR_COUNT,
//                             RT_NULL);
//        }
//
//        /* 打印所有编码器值 */
//        rt_kprintf("[%02d] Enc1: %4d | Enc2: %4d | Enc3: %4d | Enc4: %4d\n",
//                  index, counts[0], counts[1], counts[2], counts[3]);
//    }
//}


/*读取编码器的实时值，范围*/
rt_int32_t pulse_encoder_get_now_count(TIM_HandleTypeDef * htim)
{
     rt_int32_t count = 0;
     rt_enter_critical();
     //因为初值设定的是30000，所以要给予给予补偿值30000
     if(htim==&htim1) //左前
         count = (rt_int16_t)__HAL_TIM_GET_COUNTER(&htim1) - 30000;//需要根据方向加上正负号
     else if (htim==&htim2) //左后
         count = (rt_int16_t)__HAL_TIM_GET_COUNTER(&htim2) - 30000;
     else if (htim==&htim3) //右后
         count = (rt_int16_t)__HAL_TIM_GET_COUNTER(&htim3) - 30000;
     else if (htim==&htim4) //右前
         count = (rt_int16_t)__HAL_TIM_GET_COUNTER(&htim4) - 30000;
     else
         count = 0;

     rt_exit_critical();

     return count;
}

/*周期20ms的软件定时器，编码器换算速度线程*/
void encoder_calc_entry(void *parameter)
{
    static int counter = 0;
    counter++;

    // A_encoder 左前
    A_encoder.Encoder = pulse_encoder_get_now_count(&htim1);//读取A电机的编码器的实时数值
    A_encoder.overflow_count = get_encoder_overflow_count(1);//需要根据方向加上正负号
    A_encoder.total_count = A_encoder.Encoder + A_encoder.overflow_count * ENCODER_RELOAD_VALUE;
    // 单位是圈/秒
    //轮子转一圈的脉冲：(四倍频*编码器单圈脉冲*电机减速比)
    //轮子转了多少圈：(编码器差值/(四倍频*编码器单圈脉冲*电机减速比))
    //(编码器差值/(四倍频*编码器单圈脉冲*电机减速比))*测速频率（50Hz）
    A_encoder.speed = (float)(A_encoder.total_count - A_encoder.last_count)
                       / (EncoderMultiples * Hall_13 * HALL_30F) * (1000 / ENCODER_CALC_PERIOD_MS);
    A_encoder.last_count = A_encoder.total_count;

    // B_encoder 左后
    B_encoder.Encoder = pulse_encoder_get_now_count(&htim2);//读取B电机的编码器实时数值
    B_encoder.overflow_count = get_encoder_overflow_count(2);
    B_encoder.total_count = B_encoder.Encoder + B_encoder.overflow_count * ENCODER_RELOAD_VALUE;
    B_encoder.speed = (float)(B_encoder.total_count - B_encoder.last_count)
                       / (EncoderMultiples * Hall_13 * HALL_30F) * (1000 / ENCODER_CALC_PERIOD_MS);
    B_encoder.last_count = B_encoder.total_count;

    // C_encoder  右后
    C_encoder.Encoder = pulse_encoder_get_now_count(&htim3);//读取C电机的编码器实时数值
    C_encoder.overflow_count = get_encoder_overflow_count(3);
    C_encoder.total_count = C_encoder.Encoder + C_encoder.overflow_count * ENCODER_RELOAD_VALUE;
    C_encoder.speed = (float)(C_encoder.total_count - C_encoder.last_count)
                       / (EncoderMultiples * Hall_13 * HALL_30F) * (1000 / ENCODER_CALC_PERIOD_MS);
    C_encoder.speed = -C_encoder.speed;//正负暂时未确定
    C_encoder.last_count = C_encoder.total_count;

    // D_encoder  右前
    D_encoder.Encoder = pulse_encoder_get_now_count(&htim4);//读取D电机的编码器实时数值
    D_encoder.overflow_count = get_encoder_overflow_count(4);
    D_encoder.total_count = D_encoder.Encoder + D_encoder.overflow_count * ENCODER_RELOAD_VALUE;
    D_encoder.speed = (float)(D_encoder.total_count - D_encoder.last_count)
                       / (EncoderMultiples * Hall_13 * HALL_30F) * (1000 / ENCODER_CALC_PERIOD_MS);
    D_encoder.speed = -D_encoder.speed;//正负暂时未确定
    D_encoder.last_count = D_encoder.total_count;


//    rt_kprintf("[EncoderTimer] Tick: %u | Speed A: %d B: %d C: %d D: %d\n",
//               rt_tick_get(),
//               (int16_t)A_encoder.speed,
//               (int16_t)B_encoder.speed,
//               (int16_t)C_encoder.speed,
//               (int16_t)D_encoder.speed);

//    if (counter % 50 == 0) // 每 50 次（1 秒）打印一次
//    {
//        rt_kprintf("[EncoderTimer] Tick: %u | Speed A: %.2f B: %.2f C: %.2f D: %.2f\n",
//                   rt_tick_get(),
//                   A_encoder.speed,
//                   B_encoder.speed,
//                   C_encoder.speed,
//                   D_encoder.speed);
}

/*获取小车电机A的速度*/
float get_encoderA_speed(void)
{
    static float speed;
    rt_enter_critical();
    speed = A_encoder.speed;
    rt_exit_critical();
    return speed;
}

/*获取小车电机B的速度*/
float get_encoderB_speed(void)
{
    static float speed;
    rt_enter_critical();
    speed = B_encoder.speed;
    rt_exit_critical();
    return speed;
}

/*获取小车电机C的速度*/
float get_encoderC_speed(void)
{
    static float speed;
    rt_enter_critical();
    speed = C_encoder.speed;
    rt_exit_critical();
    return speed;
}

/*获取小车电机D的速度*/
float get_encoderD_speed(void)
{
    static float speed;
    rt_enter_critical();
    speed = D_encoder.speed;
    rt_exit_critical();
    return speed;
}

/*获取电机A的总脉冲*/
rt_int32_t get_A_total_count(void)
{
    static int32_t _total_count;
    rt_enter_critical();
    _total_count = A_encoder.total_count;
    rt_exit_critical();
    return _total_count;
}

/*获取电机B的总脉冲*/
rt_int32_t get_B_total_count(void)
{
    static int32_t _total_count;
    rt_enter_critical();
    _total_count = B_encoder.total_count;
    rt_exit_critical();
    return _total_count;
}

/*获取电机C的总脉冲*/
rt_int32_t get_C_total_count(void)
{
    static int32_t _total_count;
    rt_enter_critical();
    _total_count = C_encoder.total_count;
    rt_exit_critical();
    return _total_count;
}

/*获取电机D的总脉冲*/
rt_int32_t get_D_total_count(void)
{
    static int32_t _total_count;
    rt_enter_critical();
    _total_count = D_encoder.total_count;
    rt_exit_critical();
    return _total_count;
}

/*用于获取小车的移动距离*/
float get_car_distance(void)
{
    static float distance;
    rt_enter_critical();
    distance = (A_encoder.total_count) / (EncoderMultiples  * Hall_13 * HALL_30F) * Mecanum_75 * PI;//单位是米
    rt_exit_critical();
    return distance;
}


MSH_CMD_EXPORT(Encoder_init, Encoder_init);
MSH_CMD_EXPORT(ReadEncoder, ReadEncoder);
